%腿部逆运动学的解析法

function q = IK_leq(Body,D,A,B,Foot) %D躯干坐标系原点到股关节的距离，A大腿长，B小腿长
    %计算左腿只需将D变为负值
#r = Foot.R' * (Body.p + Body.R * [0 D 0]'- Foot.p);  %足部坐标系中的股关节
#C = norm(r); 
c5 = (C^2-A^2-B^2)/(2.0*A*B); 
#if c5 >= 1 
    q5 = 0.0; 
#elseif c5 <= -1 
    q5 = pi; 
#else 
    q5= acos(c5); %膝关节的俯仰角
end 
q6a = asin((A/C) * sin(pi-q5)); 
q7 = atan2(r(2),r(3)); %ankle roll -pi/2 < gl6) < pi/2 
#if q7 > pi/2, q7=q7-pi; elseif q7 < -pi/2, q7=q7+pi; end 
q6=-atan2(r(1),sign(r(3))* sqrt(r(2)^2+r(3)^2))-q6a; %踝关节的俯仰角
#R = Body.R' * Foot.R * Rroll(-q7) * Rpitch( -q6-q5);  %hipZ x hipX x hipY
q2 = atan2(-R(1,2),R(2,2)); %股关节的侧摆角
cz = cos(q2); sz = sin(q2);
q3 = atan2(R(3,2),-R(1,2)*sz + R(2,2)*cz);  %股关节的滚动角
q4 = atan2(-R(3,1),R(3,3)); %股关节的俯仰角
#q = [q2 q3 q4 q5 q6 q7]';